#ifndef _Instruction_Queue
#define _Instruction_Queue
#include <iostream>
#include <cstring>
#include <cstdio>
#include "store.hpp"
#include "arithmetic.hpp"
#include "tomasulo_cpu.hpp"

void CPU::IQ_fetch()
{
    #ifdef func
        puts("");
        puts("---IQ_fetch---");
    #endif 

    if (!ins.isissue && CPU::commit_signal >= 0)
        return;

    int tmp = CPU::pc_prev;
    if (CPU::commit_signal < 0)
        tmp = -CPU::commit_signal;

    #ifdef debug
        printf("[");
    #endif
    uint rv32i = 0;
    for (int i = 3; ~i; i--)
    {
        rv32i = rv32i << 8 | CPU::M.mem[tmp + i];
        #ifdef debug
            std::cout.fill('0');
            std::cout.width(2);
            std::cout << std::hex << (int)mem[pc + i] << ' ';
        #endif
    }

    if (rv32i == 0)
    {
        CPU::ins.isissue = 1;
        return;
    }

    #ifdef debug
        printf("]     ");
        std::cout << std::hex << pc << "       ";
        // printf("pc:%5d      ", pc);
    #endif

    CPU::ins = CPU::instruction(rv32i, tmp, 0);

    CPU::pc_new = tmp + 4;

    #ifdef func
        puts("--------------");
        puts("");
    #endif 
}

void CPU::IQ_issue()
{
    #ifdef func
        puts("");
        puts("---IQ_issue---");
    #endif 
    if (CPU::ins.isissue)
        return;

    uint rv32i = CPU::ins.rv32i, pc = CPU::ins.pc;
    bool isend = (rv32i == 0x0ff00513);

    uint opcode = rv32i & ((1 << 7) - 1);
    rv32i >>= 7;

    int rs1 = -1, rs2 = -1, rd = -1, func3 = 0, func7 = 0;
    uint imm = 0;
    if (opcode == 55) // LUI
    {
        rd = rv32i & ((1 << 5) - 1);
        rv32i >>= 5;
        imm = rv32i << 12;
    }
    else if (opcode == 23) // AUIPC
    {   
        rd = rv32i & ((1 << 5) - 1);
        rv32i >>= 5;
        imm = rv32i << 12;
    }
    else if (opcode == 111) // JAL
    {
        rd = rv32i & ((1 << 5) - 1);
        rv32i >>= 5;
        imm |= (rv32i & ((1 << 8) - 1)) << 12;
        rv32i >>= 8;
        imm |= (rv32i & 1) << 11;
        rv32i >>= 1;
        imm |= (rv32i & ((1 << 10) - 1)) << 1;
        rv32i >>= 10;
        imm |= rv32i << 20;

        imm = sext(imm, 20);
    }
    else if (opcode == 103) // JALR
    {
        rd = rv32i & ((1 << 5) - 1);
        rv32i >>= 5;

        func3 = rv32i & ((1 << 3) - 1);
        rv32i >>= 3;

        rs1 = rv32i & ((1 << 5) - 1);
        rv32i >>= 5;

        imm |= rv32i;
        
        imm = sext(imm, 11);
    }
    else if (opcode == 99) // BEQ ~ BGEU
    {
        imm |= (rv32i & 1) << 11;
        rv32i >>= 1;
        imm |= (rv32i & ((1 << 4) - 1)) << 1;
        rv32i >>= 4;

        func3 = rv32i & ((1 << 3) - 1);
        rv32i >>= 3;

        rs1 = rv32i & ((1 << 5) - 1);
        rv32i >>= 5;

        rs2 = rv32i & ((1 << 5) - 1);
        rv32i >>= 5;

        imm |= (rv32i & ((1 << 6) - 1)) << 5;
        rv32i >>= 6;

        imm |= (rv32i & 1) << 12;

        imm = sext(imm, 12);
    }
    else if (opcode == 3) // LB ~ LHU
    {
        rd = rv32i & ((1 << 5) - 1);
        rv32i >>= 5;

        func3 = rv32i & ((1 << 3) - 1);
        rv32i >>= 3;

        rs1 = rv32i & ((1 << 5) - 1);
        rv32i >>= 5;

        imm |= rv32i;

        imm = sext(imm, 11);
    }
    else if (opcode == 35) // SB ~ SW
    {
        imm |= rv32i & ((1 << 5) - 1);
        rv32i >>= 5;

        func3 = rv32i & ((1 << 3) - 1);
        rv32i >>= 3;

        rs1 = rv32i & ((1 << 5) - 1);
        rv32i >>= 5;

        rs2 = rv32i & ((1 << 5) - 1);
        rv32i >>= 5;

        imm |= rv32i << 5;

        imm = sext(imm, 11);
    }
    else if (opcode == 19) // ADDI ~ SRAI
    {
        rd = rv32i & ((1 << 5) - 1);
        rv32i >>= 5;

        func3 = rv32i & ((1 << 3) - 1);
        rv32i >>= 3;

        rs1 = rv32i & ((1 << 5) - 1);
        rv32i >>= 5;

        if (func3 == 1 || func3 == 5) // SLLI ~ SRAI
        {
            imm |= rv32i & ((1 << 5) - 1);
            rv32i >>= 5;

            func7 = rv32i;
        }
        else // ADDI ~ ANDI
        {
            imm |= rv32i;

            imm = sext(imm, 11);
        }
    }
    else if (opcode == 51) // ADD ~ AND
    {
        rd = rv32i & ((1 << 5) - 1);
        rv32i >>= 5;

        func3 = rv32i & ((1 << 3) - 1);
        rv32i >>= 3;

        rs1 = rv32i & ((1 << 5) - 1);
        rv32i >>= 5;

        rs2 = rv32i & ((1 << 5) - 1);
        rv32i >>= 5;

        func7 = rv32i;
    }

    // L or S
    if (opcode == 3 || opcode == 35)
    {
        if (CPU::LSB_prev.h.full() || CPU::ROB_prev.h.full())
            return;
        else
            CPU::ins.isissue = 1;

        Reorder_Buffer::node ROBnode(opcode, func3, func7);
        ROBnode.dest = rd;
        ROBnode.ready = 0;
        ROBnode.end = isend;
        int b = CPU::ROB_prev.h.next_pos();

        Load_Store_Buffer::node LSBnode(opcode, func3, func7);
        if (opcode == 3)
        {
            if (~CPU::R_prev.reg_state[rs1])
            {
                int h = CPU::R_prev.reg_state[rs1];
                if (CPU::ROB_prev.h.node[h].ready)
                    LSBnode.Vj = CPU::ROB_prev.h.node[h].val;
                else
                    LSBnode.Qj = h;
            }
            else
                LSBnode.Vj = CPU::R_prev.reg[rs1];

            LSBnode.A = imm;
            LSBnode.dest = b;

            CPU::R_prev.reg_state[rd] = b;
        }
        else
        {
            if (~CPU::R_prev.reg_state[rs1])
            {
                int h = CPU::R_prev.reg_state[rs1];
                if (CPU::ROB_prev.h.node[h].ready)
                    LSBnode.Vj = CPU::ROB_prev.h.node[h].val;
                else
                    LSBnode.Qj = h;
            }
            else
                LSBnode.Vj = CPU::R_prev.reg[rs1];

            if (~CPU::R_prev.reg_state[rs2])
            {
                int h = CPU::R_prev.reg_state[rs2];
                if (CPU::ROB_prev.h.node[h].ready)
                    LSBnode.Vk = CPU::ROB_prev.h.node[h].val;
                else
                    LSBnode.Qk = h;
            }
            else
                LSBnode.Vk = CPU::R_prev.reg[rs2];

            LSBnode.A = imm;
            LSBnode.dest = b;
        }

        CPU::issue_to_ROB = ROBnode;
        CPU::issue_to_LSB = LSBnode;
    }
    else
    {
        if (CPU::RS_prev.h.full() || CPU::ROB_prev.h.full())
            return;
        else
            CPU::ins.isissue = 1;

        Reorder_Buffer::node ROBnode(opcode, func3, func7);
        ROBnode.dest = rd;
        ROBnode.ready = 0;
        ROBnode.end = isend;
        int b = CPU::ROB_prev.h.next_pos();

        Reservation_Station::node RSnode(opcode, func3, func7);
        if (~rs1)
        {
            if (~CPU::R_prev.reg_state[rs1])
            {
                int h = CPU::R_prev.reg_state[rs1];
                if (CPU::ROB_prev.h.node[h].ready)
                    RSnode.Vj = CPU::ROB_prev.h.node[h].val;
                else
                    RSnode.Qj = h;
            }
            else
                RSnode.Vj = CPU::R_prev.reg[rs1];
        }

        if (~rs2)
        {
            if (~CPU::R_prev.reg_state[rs2])
            {
                int h = CPU::R_prev.reg_state[rs2];
                if (CPU::ROB_prev.h.node[h].ready)
                    RSnode.Vk = CPU::ROB_prev.h.node[h].val;
                else
                    RSnode.Qk = h;
            }
            else
                RSnode.Vk = CPU::R_prev.reg[rs2];
        }

        RSnode.A = imm;
        RSnode.dest = b;
        RSnode.pc = pc;

        CPU::issue_to_reg = std::make_pair(rd, b);

        CPU::issue_to_ROB = ROBnode;
        CPU::issue_to_RS = RSnode;
    }

    #ifdef func
        puts("issue_to_ROB");
        CPU::issue_to_ROB.print();
        puts("issue_to_RS");
        CPU::issue_to_RS.print();
        puts("issue_to_LSB");
        CPU::issue_to_LSB.print();
        puts("--------------");
        puts("");
    #endif 
};

#endif